#include <ros/ros.h>  
#include <std_msgs/Int8.h> 
#include <std_msgs/Float32.h> 

#include <balloon_rl/safe_serial.hpp>
#include "balloon_rl/serial_protocol.hpp"
#include "balloon_rl/data_classifier.hpp"


SafeSerial::SafeSerialHandle ssport;
SerialProtocol::data_unpack msgUnpacker;
SerialProtocol::data_pack msgPacker;

int looprate;
ros::Time last_cmd_time;

balloon_rl::nx_to_mcu mcuMsg_vary;

uint64_t timeStamp = 0;

uint32_t FloatToU32(float dat) {
    uint8_t buf[4];
    
    buf[0] = ((uint8_t*)&dat)[0];
    buf[1] = ((uint8_t*)&dat)[1];
    buf[2] = ((uint8_t*)&dat)[2];
    buf[3] = ((uint8_t*)&dat)[3];
    
    return (buf[0] << 24) + (buf[1] << 16) + (buf[2] << 8) + buf[3];
}

void node_param_init(ros::NodeHandle &n)
{
    std::string serial_port;
    int bandrate;
    n.getParam("serial_port", serial_port);
    n.getParam("bandrate", bandrate);
    n.getParam("looprate", looprate);

    ssport.setport(serial_port, bandrate);
    ssport.init();
}

// void Callback(const balloon_rl::nx_to_mcu &msg)
void Callback(const std_msgs::Float32 &msg)
{
    // ROS_INFO("[serial_send_node] float: %f", msg.data);
    mcuMsg_vary.timeStamp = timeStamp++;
    mcuMsg_vary.thrust = FloatToU32(msg.data); 
    msgPacker.ID = DataClassifier::RL_ACTION_MSG;
    msgPacker.dataLen = RL_ACTION_LEN;
    // ROS_INFO("[serial_send_node] U32: %d", mcuMsg_vary.thrust);

    msgPacker.set_header(RL_ACTION_LEN, DataClassifier::RL_ACTION_MSG);

    std::vector<uint8_t> cmd_buff(RL_ACTION_LEN);
    DataClassifier::McuData_pack(&mcuMsg_vary, cmd_buff);
    if(msgPacker.set_buff(cmd_buff) == true)
    {
        msgPacker.set_crc();
        if(ssport.check())
        {
            ssport.write(&msgPacker.packbuff[0] , msgPacker.packbuff.size());
        }
    }
    else
    {
        printf("acc cmd pack error\r\n");
    }
}

void msg_reset(){
    msgPacker.ID = DataClassifier::NO_ID;
    msgPacker.dataLen = 0;
    msgPacker.msgSerial_freeMomory();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "nx_serial_send_node");
    ros::NodeHandle n("~");
    ros::NodeHandle n_;
    node_param_init(n);

    ros::Rate loop_rate(looprate);

    ros::Subscriber sub_com = n_.subscribe("MCU_DATA_send", 10, Callback, ros::TransportHints().tcpNoDelay());

    ROS_INFO("serial_send_node create!");

    while (ros::ok())
    {
        ros::spinOnce();
        if(ssport.check())
        {
            ROS_INFO("send node msgID: %x", msgPacker.ID);
            
            if(msgPacker.ID != DataClassifier::NO_ID && msgPacker.msgSerial_init()){
                switch(msgPacker.ID){
                    case DataClassifier::RL_ACTION_MSG:
                        DataClassifier::McuData_pack(&mcuMsg_vary, msgPacker.fullData);
                        break;
                        
                    default:
                        break;
                }

                msgPacker.msgSerial_crc16Check_Data();
                ssport.write(msgPacker.fullData, msgPacker.fullLen);
                msg_reset();
            }
        }

        loop_rate.sleep();
    }

    ssport.close();
    
    return 0;
}